#include "INC.h"

TrackerData tracker_data = {0};

uint8_t volatile adc_conversion_flag;

void analog_tracker_init(void)
{
    // 初始化模拟巡线传感器相关的硬件设置
    // 例如配置ADC引脚，初始化ADC模块等
    NVIC_EnableIRQ(ADC12_TRACKER_INST_INT_IRQN);
    S0(0);
    S1(0);
    S2(0);
    adc_conversion_flag = 0;
    tracker_data.delta_threshold_dead_zone = 50;
}

void get_analog_tracker_values(TrackerData *values)
{
    // 读取模拟巡线传感器的值
    // 例如从ADC读取值并存储到data结构中
    for (int i = 0; i < 8; i++)
    {
        values->analog_sensors[i] = tracker_data.analog_sensors[i];
    }
}

/* ========= 快速 Otsu（8位、整型实现） =========
 * 输入：vals[0..n-1]（0..255），n>0
 * 算法：构建 0..255 直方图；仅在 [vmin, vmax) 里扫描；
 *       用整数形式最大化类间方差的无关常数化“分数”：
 *       score = ((T * sumB - wB * sumT)^2) / (wB * (T - wB))，T=总样本数。
 * 说明：这里比较 score（64位整数/除法）即可，不会溢出。
 */
static uint8_t otsu_threshold_u8_fast(uint8_t *vals, uint8_t n)
{


    // 预期 n <= 8
    static uint8_t hist[256] = {0};
    // 将 hist 清零
    for (int i = 0; i < 256; i++)
    {
        hist[i] = 0;
    }
    uint8_t vmin = 255, vmax = 0;
    uint16_t sumT = 0; // Σ vals[i]，<= 2040
    uint8_t T = n;

    for (uint8_t i = 0; i < n; ++i)
    {
        uint8_t v = vals[i];
        hist[v] += 1; // 每箱计数 <= 8
        if (v < vmin)
            vmin = v;
        if (v > vmax)
            vmax = v;
        sumT += v;
    }
    if (abs(vmax - vmin) <= tracker_data.delta_threshold_dead_zone){

    
        tracker_data.delta_threshold = abs(vmax - vmin);
        return vmax; // 全部相等
    }

    uint8_t best_t = vmin;
    uint8_t wB = 0;         // <= 8
    uint16_t sumB = 0;      // <= 2040
    uint32_t best_num2 = 0; // 保存 (num)^2
    uint8_t best_den = 1;   // 保存 den = wB*wF (1..16)

    for (uint16_t t = vmin; t < vmax; ++t)
    {
        uint8_t h = hist[t];
        if (h)
        {
            wB += h;
            sumB += (uint16_t)h * (uint16_t)t; // h*t <= 2040
        }
        if (wB == 0)
            continue;
        if (wB == T)
            break; // 前景为空

        uint8_t wF = (uint8_t)(T - wB);
        uint8_t den = (uint8_t)(wB * wF); // <= 16

        // num = T*sumB - wB*sumT，范围 [-16320,16320]
        int32_t num = (int32_t)T * (int32_t)sumB - (int32_t)wB * (int32_t)sumT;
        uint32_t num2 = (uint32_t)(num < 0 ? -num : num);
        num2 *= num2; // <= 266,342,400

        // 比较 num2/den 与 best_num2/best_den（交叉相乘，避免除法和精度损失）
        if ((uint32_t)num2 * (uint32_t)best_den > (uint32_t)best_num2 * (uint32_t)den)
        {
            best_num2 = num2;
            best_den = den;
            best_t = (uint8_t)t;
        }
    }
    return best_t;
}

void update_tracker_values(void)
{

    // 1) 采集 8 路 ADC（本身 8 位），保存到 analog_sensors
    S0(0);
    S1(0);
    S2(0);

    DL_ADC12_startConversion(ADC12_TRACKER_INST);
    while (adc_conversion_flag == 0)
        ;
    adc_conversion_flag = 0;
    tracker_data.analog_sensors[0] = DL_ADC12_getMemResult(ADC12_TRACKER_INST, ADC12_TRACKER_ADCMEM_0);

    for (int i = 0; i < 8; i++)
    {
        S0(i & 0x01);
        S1((i >> 1) & 0x01);
        S2((i >> 2) & 0x01);

        DL_ADC12_startConversion(ADC12_TRACKER_INST);
        while (adc_conversion_flag == 0)
            ;
        adc_conversion_flag = 0;
        tracker_data.analog_sensors[i] = DL_ADC12_getMemResult(ADC12_TRACKER_INST, ADC12_TRACKER_ADCMEM_0);
    }
    // 2) 计算 Otsu 阈值（基于本次 8 个 8位值）
    tracker_data.threshold = otsu_threshold_u8_fast(tracker_data.analog_sensors, 8);

    // 3) 生成 digital_sensors：> 阈值为 1，≤ 阈值为 0
    for (int i = 0; i < 8; i++)
    {
        tracker_data.digital_sensors[i] = (tracker_data.analog_sensors[i] > tracker_data.threshold) ? 1u : 0u;
    }
}

void get_digital_tracker_values(TrackerData *values)
{

    // 根据模拟传感器值和阈值计算数字传感器值
    for (uint8_t i = 0; i < 8; i++)
    {
        values->digital_sensors[i] = tracker_data.digital_sensors[i];
    }
}

void ADC12_TRACKER_INST_IRQHandler(void)
{
    // 处理ADC中断，读取传感器值并存储到analog_tracker_data中
    // 清除中断标志
    switch (DL_ADC12_getPendingInterrupt(ADC12_TRACKER_INST))
    {
    case DL_ADC12_IIDX_MEM0_RESULT_LOADED:
        adc_conversion_flag = 1;
        break;

    default:
        break;
    }
}
